#include "videocapture.h"
#include "extension/colors.h"

#include <iostream>
#include <cerrno>
#include <cstring>
#include <fcntl.h>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>
#include <asm/types.h>

using namespace bend2d;
using namespace std;

#define TRACE(x)        cout << dec << __PRETTY_FUNCTION__ << " | " << x << "\n"
#define TRACE_ERROR(x)  cerr << dec << __PRETTY_FUNCTION__ << " | " << x << "\n"
#define TRACE_PERROR(x) cerr << dec << __PRETTY_FUNCTION__ << " | " << x << " (" << errno << ") - " << strerror(errno) << "\n"
#define CLEAR(x) memset (&(x), 0, sizeof (x))

int xioctl (int fd, int request, void * arg)
{
    int r;
    do r = ioctl (fd, request, arg);
    while (-1 == r && EINTR == errno);
    return r;
}

videoCapture::videoCapture() :
    m_IOMethod(IO_METHOD_READ),
    m_Buffers(NULL),
    m_SkipReading(false)
{
}

videoCapture::~videoCapture()
{
    delete m_Buffers;
}

void videoCapture::SkipReading(bool value)
{
    m_SkipReading = value;
}

bool videoCapture::openVideoDevice(const string& dev)
{
    struct stat st;
    m_Device = dev;
    string deviceadd = "/dev/"+dev;

    if (-1 == stat (deviceadd.c_str(), &st))
    {
        TRACE_PERROR("Cannot identify device " << dev);
        return false;
    }

    if (!S_ISCHR (st.st_mode))
    {
        TRACE_ERROR(dev << " is not a device");
        return false;
    }

    m_DevFD = open (deviceadd.c_str(), O_RDWR | O_NONBLOCK, 0);
    if (-1 == m_DevFD)
    {
        TRACE_PERROR("Cannot open device " << dev);
        return false;
    }

    return true;
}

bool videoCapture::initDevice()
{
    struct v4l2_capability cap;
    struct v4l2_cropcap cropcap;
    struct v4l2_crop crop;
    struct v4l2_format fmt;
    unsigned int min;

    if (-1 == xioctl (m_DevFD, VIDIOC_QUERYCAP, &cap)) 
    {
        TRACE_PERROR(m_Device << " it not a V4L2 device");
        return false;
    }

    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) 
    {
        TRACE_ERROR(m_Device << " is not a video capture device");
        return false;
    }

    switch (m_IOMethod) 
    {
        case IO_METHOD_READ:
            if (!(cap.capabilities & V4L2_CAP_READWRITE)) 
            {
                TRACE_ERROR(m_Device << " does not support read i/o");
                return false;
            }
            break;
        default:
            TRACE_ERROR("Not supported io method");
            return false;
    }


    /* Select video input, video standard and tune here. */
    /* TODO Check this part */
    CLEAR (cropcap);
    cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (0 == xioctl (m_DevFD, VIDIOC_CROPCAP, &cropcap)) 
    {
        crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        crop.c = cropcap.defrect; /* reset to default */

        if (-1 == xioctl (m_DevFD, VIDIOC_S_CROP, &crop)) 
        {
            TRACE_PERROR("Cropping not supported");
            return false;
        }
    } 
    else 
    {  
        /* Errors ignored. */
    }

    // It doesnt work for PS3 Eye Driver //
    CLEAR (fmt);
    fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width       = 640; 
    fmt.fmt.pix.height      = 480;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB565;
    fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;

    if (xioctl (m_DevFD, VIDIOC_S_FMT, &fmt))
    {
        TRACE_PERROR("Fail to set FMT");
        return false;
    }
    else
    {
        TRACE("Video initialized");
        TRACE("        Type: " << fmt.type);
        TRACE("       Width: " << fmt.fmt.pix.width);
        TRACE("      Height: " << fmt.fmt.pix.height);
        TRACE(" PixelFormat: " << fmt.fmt.pix.pixelformat);
        TRACE("       Field: " << fmt.fmt.pix.field);
        TRACE("        Size: " << fmt.fmt.pix.sizeimage);
    }


    switch (m_IOMethod) {
        case IO_METHOD_READ:
            initRead (fmt.fmt.pix.sizeimage);
            break;

        default:
            TRACE_ERROR("Not supported io method");
            return false;
    }

    return true;
}

void videoCapture::initRead (unsigned int buffer_size)
{
    m_Buffers = new buffer(buffer_size, buffer_size * 2, 16, 32);

    if (!m_Buffers)
        TRACE_ERROR("Out of memory");

    if (!m_Buffers[0].cbuff)
        TRACE_ERROR("Out of memory");
}

int videoCapture::readFrame ()
{
    static unsigned int readsize = 0;

    if(m_SkipReading)
    {
        convertBuffer(m_Buffers);
        return 1;
    }
  
    switch (m_IOMethod)
    {
        case IO_METHOD_READ:
            if ((readsize = read (m_DevFD, m_Buffers[0].buff, m_Buffers[0].buffsize)) == -1)
            {
                switch (errno)
                {
                    case EAGAIN:
                        return -1;

                    case EIO:
                        return -1;

                    default:
                        TRACE_PERROR("Reading fail");
                        return 0;
                }
            }

            if(readsize != m_Buffers[0].buffsize)
                return -1;

            convertBuffer(m_Buffers);
            return 1;
    }
    return 0;
}

const Color& videoCapture::getPixelColor(int x, int y) const
{
    return m_Buffers->colorbuff[y*640 + x];
}

const Color& videoCapture::getPixelColor(int pos) const
{
    return m_Buffers->colorbuff[pos];
}

void videoCapture::convertBuffer(buffer* ptr)
{
    for(size_t i = 0; i < ptr->buffsize; i += 4)
    {
        unsigned char v = ptr->buff[i+3];
        unsigned char y2 = ptr->buff[i+2];
        unsigned char u = ptr->buff[i+1];
        unsigned char y1 = ptr->buff[i+0];

        unsigned int coloryuv1 = y1;
        coloryuv1 = (coloryuv1 << 8) | u;
        coloryuv1 = (coloryuv1 << 8) | v;
        coloryuv1 = (coloryuv1 << 8) | 0xFF;

        unsigned int coloryuv2 = y2;
        coloryuv2 = (coloryuv2 << 8) | u;
        coloryuv2 = (coloryuv2 << 8) | v;
        coloryuv2 = (coloryuv2 << 8) | 0xFF;

        Color color1(coloryuv1, Color::eSourceTypeYCbCr);
        Color color2(coloryuv2, Color::eSourceTypeYCbCr);

        filter(i/2, color1);
        filter(i/2 + 1, color2);
        ptr->uicbuff[i/2] = color1.To8888();
        ptr->uicbuff[i/2+1] = color2.To8888();
        ptr->colorbuff[i/2] = color1;
        ptr->colorbuff[i/2 + 1] = color2;
    }
}

